/*
 * CvApplication.cpp
 *
 *  Created on: 2018年3月20日
 *      Author: zhengboyuan
 */

#include "CvApplication.h"
#include "Path.h"
#include "TStringCast.h"
#include "TStringUtil.h"
#include <algorithm>
#include <functional>

#include "HeadCounter.h"

void trace(const char* fmt, ...)
{
	va_list ap;
	va_start(ap, fmt);

	char buffer[1024];
	int len = vsprintf_s(buffer, 1024, fmt, ap);
	if (len >= 0)
	{
		buffer[len] = '\0';

		OutputDebugString(buffer);
	}

	va_end(ap);
}

void CvApplication::TrackbarCallback(int pos, void* userdata)
{
	CvApplication* app = (CvApplication*)userdata;
	app->onTrack();
}

void CvApplication::MouseCallback(int event, int x, int y, int flags, void* param)
{
	CvApplication* app = (CvApplication*)param;
	app->onMouse(event, x, y, flags);
}


CvApplication::CvApplication():
	m_srcWindow("src"),
	m_outWindow("out"),
	m_watchWindow("watch"),
	m_hTrack("hue", m_outWindow.name(), 40, 255, TrackbarCallback, this),
	m_sTrack("sat", m_outWindow.name(), 40, 255, TrackbarCallback, this),
	m_vTrack("val", m_outWindow.name(), 3, 3, TrackbarCallback, this),
	m_hdTrack("h delta", m_outWindow.name(), 1, 255, TrackbarCallback, this),
	m_sdTrack("s delta", m_outWindow.name(), 1, 255, TrackbarCallback, this),
	m_vdTrack("v delta", m_outWindow.name(), 200, 255, TrackbarCallback, this),
	m_arcTrack("arc", m_srcWindow.name(), 10, 100, TrackbarCallback, this),
	m_areaTrack("area*100", m_srcWindow.name(), 1, 100, TrackbarCallback, this),
	m_radiusTrack("radius", m_srcWindow.name(), 40, 100, TrackbarCallback, this)
{
	m_srcWindow.setMouseCallback(MouseCallback, this);
	m_outWindow.setMouseCallback(MouseCallback, this);
}

CvApplication::~CvApplication()
{
}

void CvApplication::onTrack()
{
	process();
}

void CvApplication::onMouse(int event, int x, int y, int flags)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		cv::Point point(x, y);
		cv::Vec3b color = m_srcMat.at<cv::Vec3b>(point);

		trace("%d, %d, %d\n", color[0], color[1], color[2]);
	}
}


cv::Mat readImage(const char* filepath)
{
	cv::Mat srcMat = cv::imread(filepath);

	if (srcMat.size().width > 1024)
	{
		cv::Mat resizeMat;
		cv::resize(srcMat, resizeMat, cv::Size(640, 480));
		srcMat = resizeMat;
	}
	
	return srcMat;
}

void drawNum(cv::Mat& mat, int count, cv::Scalar color = cv::Scalar(255, 255, 0))
{
	std::string text = comn::StringUtil::format("%d", count); // comn::StringCast::toString(count);
	int fontFace = cv::FONT_HERSHEY_SIMPLEX;
	double fontScale = 2;
	cv::Point point(mat.size().width / 2, mat.size().height / 2);
	cv::putText(mat, text, point, fontFace, fontScale, color, 2);
}


void CvApplication::process()
{
	cv::Mat srcMat = m_srcMat.clone();
	cv::Mat outMat = process(srcMat);

	if (srcMat.dims > 2)
	{
		cv::Mat rgbMat;
		cvtColor(srcMat, rgbMat, CV_HSV2BGR);
		srcMat = rgbMat;
	}
	
	cv::imshow(m_srcWindow.name(), srcMat);

	cv::imshow(m_outWindow.name(), outMat);
}

void CvApplication::setup()
{
	std::string workdir = comn::Path::getWorkDir();
	std::string assets = comn::Path::join(workdir, "..\\assets\\test");
	m_videoSource.setPath(assets.c_str());
	m_videoSource.setSize(cv::Size(640, 480));

	std::string filename = comn::Path::join(assets, "20180319092915972.jpg");
	//m_originMat = readImage(filename.c_str());
	//cvtColor(m_originMat, m_srcMat, CV_BGR2HSV);

	m_videoSource.getFrame(m_originMat);
	m_videoSource.getFrame(m_originMat);
	m_videoSource.getFrame(m_originMat);
	if (m_videoSource.getFrame(m_originMat))
	{
		cvtColor(m_originMat, m_srcMat, CV_BGR2HSV);
	}

	process();
}

void CvApplication::teardown()
{
	//
}

void CvApplication::processNext()
{
	if (m_videoSource.getFrame(m_originMat))
	{
		cvtColor(m_originMat, m_srcMat, CV_BGR2HSV);
	}

	process();
}

void CvApplication::run()
{
	setup();

	while (true)
	{
		int key = cv::waitKey();
		if (key == 'q' || key == -1)
		{
			break;
		}
		else if (key == 'n')
		{
			processNext();
		}
		else
		{

		}
	}

	teardown();
}

void drawContour(cv::Mat& mat, std::vector<std::vector<cv::Point> >& contours)
{
	cv::Scalar color(255, 0, 0);
	for (size_t i = 0; i < contours.size(); i++)
	{
		cv::drawContours(mat, contours, i, color);
	}
}

cv::Mat CvApplication::processEdge(cv::Mat& src)
{
	int v = m_vTrack.value();

	cv::Mat blurMat;
	cv::blur(src, blurMat, cv::Size(3, 3));

	int first = m_hTrack.value();
	int second = m_sTrack.value() * 2;

	cv::Mat edgeMat;
	cv::Canny(blurMat, edgeMat, first, second);

	std::vector<std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(edgeMat, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

	int minArc = m_arcTrack.value() * 4;
	int minArea = m_areaTrack.value() * 100;
	int radius = m_radiusTrack.value() * 4;

	auto contourFilter = [=](std::vector<cv::Point>& contour) -> bool
	{
		int arc = contour.size();
		double area = cv::contourArea(contour);
		cv::Rect rect = cv::boundingRect(contour);

		if (arc < minArc || area < minArea)
		{
			return true;
		}

		return false;
		//return rect.width > radius || rect.height > radius;
	};

	auto newEnd = std::remove_if(contours.begin(), contours.end(), contourFilter);
	contours.erase(newEnd, contours.end());

	int count = contours.size();

	cv::Mat outlineMat = cv::Mat::zeros(src.size(), CV_8UC1);

	cv::Scalar color(255, 0, 0);
	for (size_t i = 0; i < contours.size(); i++)
	{
		cv::drawContours(outlineMat, contours, i, color);
	}

	cv::blur(outlineMat, src, cv::Size(v, v));

	cv::Scalar lowbound(1, 1, 1);
	cv::Scalar upbound(255, 255, 255);
	cv::inRange(src, lowbound, upbound, outlineMat);

	

	src = outlineMat;

	drawNum(src, count);

	return edgeMat;
}



int findColorContours(cv::Mat& srcMat, cv::Scalar lowColor, cv::Scalar delta,
	std::vector<std::vector<cv::Point> >& outContours)
{
	cv::Scalar lowbound = lowColor;
	cv::Scalar upbound;
	cv::add(lowColor, delta, upbound);


	cv::Mat binMat = cv::Mat::zeros(srcMat.size(), CV_8UC1);
	cv::inRange(srcMat, lowbound, upbound, binMat);

	std::vector<std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(binMat, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	for (size_t i = 0; i < contours.size(); ++i)
	{
		outContours.push_back(contours[i]);
	}

	return contours.size();
}

cv::Mat CvApplication::processColor(cv::Mat& src)
{
	cv::Scalar lowColor(m_hTrack.value(), m_sTrack.value(), m_vTrack.value());
	cv::Scalar delta(m_hdTrack.value(), m_sdTrack.value(), m_vdTrack.value());

	int minArc = m_arcTrack.value();
	int minArea = m_areaTrack.value() * 100;
	int radius = m_radiusTrack.value();

	cv::Mat maskMat = makeForeground(src);

	cv::Mat cutMat;
	src.copyTo(cutMat, maskMat);

	cv::Mat blurMat;
	cv::blur(cutMat, blurMat, cv::Size(5, 5));

	cv::Mat outlineMat = cv::Mat::zeros(src.size(), CV_8UC1);

	std::vector<std::vector<cv::Point> > contours;
	int count = 0;

	count += findColorContours(blurMat, lowColor, delta, contours);

	drawContour(outlineMat, contours);


	auto contourFilter = [=](std::vector<cv::Point>& contour) -> bool
	{
		int arc = contour.size();
		double area = cv::contourArea(contour);
		cv::Rect rect = cv::boundingRect(contour);

		if (arc < minArc || area < minArea)
		{
			return true;
		}

		return false;
		//return rect.width > radius || rect.height > radius;
	};

	auto newEnd = std::remove_if(contours.begin(), contours.end(), contourFilter);
	contours.erase(newEnd, contours.end());

	count = contours.size();

	drawContour(src, contours);
	drawNum(src, count);

	return outlineMat;
}

cv::Mat CvApplication::process(cv::Mat& src)
{
	return processErode(src);
	//return processForeground(src);
	//return processLine(src);
	//return processColor(src);
	//return processHeadCounter(src);
}

cv::Mat CvApplication::makeForeground(cv::Mat& src)
{
	return makeForeground(src, 70, 70 * 3, 20);
}

cv::Mat CvApplication::makeForeground(cv::Mat& src, double lowThresh, double highThresh, int minRadius)
{
	cv::Mat blurMat;
	cv::blur(src, blurMat, cv::Size(3, 3));

	cv::Mat binMat;
	cv::cvtColor(blurMat, binMat, CV_BGR2GRAY);

	cv::Mat edgeMat;
	cv::Canny(binMat, edgeMat, lowThresh, highThresh);

	cv::Mat rectMat = makeContourRectMat(edgeMat, 0);
	return makeContourRectMat(rectMat, minRadius);
}

cv::Mat CvApplication::makeContourRectMat(cv::Mat& binMat, int minRadius)
{
	std::vector<std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarchy;

	cv::findContours(binMat, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	cv::Mat rectMat = cv::Mat::zeros(binMat.size(), CV_8UC1);
	cv::Scalar color(255, 0, 0);
	for (auto contour : contours)
	{
		cv::Rect rect = cv::boundingRect(contour);

		if (rect.width < minRadius || rect.height < minRadius)
		{
			continue;
		}

		cv::Point points[4] = { rect.tl() , rect.tl() , rect.br(), rect.br() };
		cv::CvUtil::getPoints(rect, points);

		cv::fillConvexPoly(rectMat, points, 4, color);
	}
	return rectMat;
}


cv::Mat CvApplication::processHeadCounter(cv::Mat& src)
{
	std::string workdir = comn::Path::getWorkDir();
	std::string filename = comn::Path::join(workdir, "HeadCounter.ini");
	
	HeadCounter headCounter;
	headCounter.loadParams(filename.c_str());

	cv::Mat rgbMat = m_originMat.clone();

	std::vector<std::vector<cv::Point> > contours;
	int count = 0;
	cv::Mat outMat = headCounter.countHead(rgbMat, count, contours);

	cv::CvUtil::filterRectRatio(contours, 0, 3);
	cv::CvUtil::filterRect(contours, 40);

	cv::CvUtil::distinctContours(src.size(), contours);

	count = contours.size();

	drawContour(src, contours);
	drawNum(src, count);

	//for (size_t i = 0; i < contours.size(); i++)
	//{
	//	//cv::drawContours(binMat, contours, i, color);
	//	cv::Scalar color(255, 0, 0);
	//	cv::fillConvexPoly(src, contours[i], color);
	//}

	return outMat;
}

cv::Mat CvApplication::processLine(cv::Mat& src)
{
	cv::Scalar lowColor(m_hTrack.value(), m_sTrack.value(), m_vTrack.value());
	cv::Scalar delta(m_hdTrack.value(), m_sdTrack.value(), m_vdTrack.value());

	int minArc = m_arcTrack.value();
	int minArea = m_areaTrack.value() * 100;
	int radius = m_radiusTrack.value();

	//cv::Mat maskMat = makeForeground(src);

	//cv::Mat cutMat;
	//src.copyTo(cutMat, maskMat);

	cv::Mat blurMat;
	cv::blur(src, blurMat, cv::Size(3, 3));

	cv::Mat binMat;
	cv::cvtColor(blurMat, binMat, CV_BGR2GRAY);

	int first = m_hTrack.value();
	int second = m_sTrack.value() * 3;

	cv::Mat edgeMat;
	cv::Canny(binMat, edgeMat, first, second);

	std::vector< cv::Mat > channels;
	cv::split(src, channels);

	std::vector< cv::Mat > edges(3);
	for (size_t i = 0; i < 3; ++i)
	{
		cv::Mat tmp;
		cv::blur(channels[i], tmp, cv::Size(3, 3));

		cv::Canny(tmp, edges[i], first, second);
	}
	
	cv::Mat objMat = edges[1] + edges[2] + edgeMat;

	src = objMat;

	/*
	std::vector<cv::Vec4i> lines;
	double rho = delta[0];
	double theta = delta[1] * CV_PI / 180;
	cv::HoughLinesP(objMat, lines, rho, theta, 80, minArc);
	for (size_t i = 0; i < lines.size(); i++)
	{
		cv::Vec4i l = lines[i];
		cv::line(src, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255, 255, 255), 1, CV_AA);
	}

	int count = lines.size();
	drawNum(src, count);
	*/

	return edgeMat;
}

cv::Mat CvApplication::processForeground(cv::Mat& src)
{
	cv::Scalar lowColor(m_hTrack.value(), m_sTrack.value(), m_vTrack.value());
	int radius = m_radiusTrack.value();

	cv::Mat blurMat;
	cv::blur(src, blurMat, cv::Size(3, 3));

	cv::Mat binMat;
	cv::cvtColor(blurMat, binMat, CV_BGR2GRAY);

	cv::Mat edgeMat;
	cv::Canny(binMat, edgeMat, lowColor[0], lowColor[1] * 3);

	src = edgeMat;

	cv::Mat rectMat = makeContourRectMat(edgeMat, 0);
	return makeContourRectMat(rectMat, radius);

	return rectMat;
}

cv::Mat CvApplication::processErode(cv::Mat& src)
{
	cv::Scalar lowColor(m_hTrack.value(), m_sTrack.value(), m_vTrack.value());
	cv::Scalar delta(m_hdTrack.value(), m_sdTrack.value(), m_vdTrack.value());

	int minArc = m_arcTrack.value();
	int minArea = m_areaTrack.value() * 100;
	int radius = m_radiusTrack.value();

	//cv::Mat maskMat = makeForeground(src);

	//cv::Mat cutMat;
	//src.copyTo(cutMat, maskMat);

	cv::Mat blurMat;
	cv::blur(src, blurMat, cv::Size(3, 3));

	cv::Mat binMat;
	cv::cvtColor(blurMat, binMat, CV_BGR2GRAY);

	int first = m_hTrack.value();
	int second = m_sTrack.value() * 3;

	cv::Mat edgeMat;
	cv::Canny(binMat, edgeMat, first, second);

	std::vector< cv::Mat > channels;
	cv::split(src, channels);
	if (lowColor[2] < 3)
	{
		int index = lowColor[2];
		cv::Canny(channels[index], edgeMat, first, second);
	}

	cv::Mat erodeMat;
	cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
	cv::morphologyEx(edgeMat, erodeMat, cv::MORPH_GRADIENT, kernel);
	//cv::dilate(edgeMat, erodeMat, kernel);

	cv::imshow(m_watchWindow.name(), erodeMat);

	Contours contours;
	src = makeContours(erodeMat, minArc, minArea, contours);


	return edgeMat;
}

cv::Mat CvApplication::makeContours(cv::Mat& binMat, int minArc, double minArea, Contours& contours)
{
	std::vector<cv::Vec4i> hierarchy;
	cv::findContours(binMat, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	cv::CvUtil::filterArc(contours, minArc);
	cv::CvUtil::filterArea(contours, minArea);

	cv::Mat out = cv::Mat::zeros(binMat.size(), CV_8UC1);
	drawContour(out, contours);
	return out;
}